package org.usfirst.frc.team4188.robot.subsystems;

import org.usfirst.frc.team4188.robot.RobotMap;
import org.usfirst.frc.team4188.robot.commands.ManualDrive;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 */
public class DriveTrain extends Subsystem {
    Encoder encoderFrontRight = RobotMap.encoderFrontRight;
    AnalogInput practicePotentiometer = RobotMap.practicePotentiometer;
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
	RobotDrive robotDrive = RobotMap.driveBase;
	CANTalon frontLeft = RobotMap.frontLeft;
	CANTalon frontRight = RobotMap.frontRight;
	CANTalon rearLeft = RobotMap.rearLeft;
	CANTalon rearRight = RobotMap.rearRight;
	static Gyro gyro = RobotMap.drivetraingyro;
   public static void init(){
	   gyro.reset();
	   
	   
    }
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    	setDefaultCommand(new ManualDrive());
    	
    }
    
    
    public void driveWithJoystick(double x, double y, double twist, double throttle, double direction){
        robotDrive.mecanumDrive_Cartesian(x*throttle, -y*throttle, twist*throttle, direction);
    }

	public double getencoderFrontRight(){
		return encoderFrontRight.getDistance();
		
	}
	
	
	public double getpracticePotentiometer(){
		return practicePotentiometer.getValue();
	}
	
	public void getEncoderValues(){
		SmartDashboard.putNumber("gyro", gyro.getAngle());
		SmartDashboard.putNumber("frontRightEncoder Distance", encoderFrontRight.getDistance());
	}
	
	public void resetEncoders(){
		encoderFrontRight.reset();
	}
	
	
}

